#include "robot_manager.h"
#include <sstream>
#include <vector>
#include "utilities/file.hpp"


void RobotManager::commandMoveTrajCallback(const std_msgs::String::ConstPtr &msg)
{
    std::istringstream ss(msg->data);
    ROS_INFO("move traj rec data: %s", ss.str().c_str());

    std::vector<double> mit_kp, mit_kd;
    nh_.getParam("mit_kp", mit_kp);
    nh_.getParam("mit_kd", mit_kd);

    std::vector<std::vector<double>> traj;
    // std::vector<std::vector<double>> traj = ReadData2D(ss.str(), ',');

    for (int k = 0; k < traj.size(); k++)
    {
        ROS_INFO_STREAM(traj[k][0] << ", " << traj[k][1] << ", " << traj[k][2] << ", " << traj[k][3]
                                        << ", " << traj[k][4] << ", " << traj[k][5] << ", " << traj[k][6]
                                        << ", ");
    }
    ROS_INFO("traj_pos_size: %ld", traj.size());

    //! 线性插值过程
    ros::Rate rate(CONTROL_FREQ);
    int k = 0;
    std::vector<double> q(7, 0.0), qd(7, 0.0), qdd(7, 0.0), torque;
    while (1)
    {
        if (k >= traj.size())
        {
            break;
        }

        pthread_mutex_lock(&zoros_mutex_);
        q = joint_state_msg_.position;
        robot_model_->inverseDynamics(q, qd, qdd, torque);
        for (size_t i = 0; i < 7; i++)
        {
            if (!is_simulation_)
            {
                //! 实机模式下还需调用 DM SDK 获取实际的关节角度
                // dm_->control_pos_vel(motors_[i], traj[k][i], 10.0);
                dm_->control_mit(motors_[i], mit_kp[i], mit_kd[i], traj[k][i], 0.0, torque[i]);
            }
            else
            {
                joint_state_msg_.position[i] = traj[k][i];
                //! 如果是仿真模式，发布一条初始消息，所有关节的位置为0.0
                joint_state_msg_.header.stamp = ros::Time::now();
                joint_pub_.publish(joint_state_msg_); // 发布初始的 joint_states 消息
            }
        }
        pthread_mutex_unlock(&zoros_mutex_);

        k++;
        rate.sleep();
    }

}
